#include "BitstreamReader.h"

namespace opennav {

BitstreamReader::BitstreamReader(uint8_t* data, int32_t length) :
	data_(data), length_(length), bitpos_(0)  { }

bool BitstreamReader::read_bit() {
	int32_t byte_offset = bitpos_ / 8;
	int32_t bit_offset = bitpos_ % 8;
	bitpos_++;
	return ((data_[byte_offset] >> bit_offset) & 0x1) == 1;
}

uint32_t BitstreamReader::read(int32_t nbits) {
	uint32_t value = 0;
	int32_t pos = 0;
	while(pos < nbits) {
		int32_t byte_offset = bitpos_ / 8;
		int32_t bit_offset = bitpos_ % 8;
		uint8_t b = data_[byte_offset];
		b = b >> bit_offset;
		int32_t n = nbits - pos;
		if (n > 8 - bit_offset) {
			n = 8 - bit_offset;
		}
		int32_t mask = ((1 << n) - 1);
		value |= ((b & mask) << pos);
		pos += n;
		bitpos_ += n;
	}
	return value;
}

bool BitstreamReader::can_read(int32_t nbits) const {
	int32_t byte_offset = (bitpos_ + nbits - 1) / 8;
	return byte_offset < length_;
}

int32_t BitstreamReader::bits_remaining() const {
	return (length_ * 8) - bitpos_;
}

}
